"""
目前已经实现的：
连通舵机并能给舵机发送指令运动

还缺少的：
1.首先初始化让舵机停在(0, 0)位置
"""


import sys
import time
sys.path.append("..")
import serial
import serial.tools.list_ports
from scservo_sdk import *


# 列出所有可用串口
def list_ports():
    ports = list(serial.tools.list_ports.comports())
    print("可用串口：")
    for i, port in enumerate(ports):
        print(f"{i}: {port.device}")
    return ports


# 列出可选波特率
def list_baudrates():
    baudrates = [1000000, 115200, 57600, 9600, 250000, 500000, 2000000]
    print("可用波特率：")
    for i, br in enumerate(baudrates):
        print(f"{i}: {br}")
    return baudrates


# 打开用户选择的串口
def open_port(ports, index, baudrate=1000000):
    try:
        portHandler = PortHandler(ports[index].device)  # 创建 PortHandler
        if portHandler.openPort():
            print(f"✅ 成功打开串口 {ports[index].device}")
        else:
            print("❌ 打开串口失败")
            return None

        if portHandler.setBaudRate(baudrate):
            print(f"✅ 成功设置波特率为 {baudrate}")
        else:
            print("❌ 波特率设置失败")
            return None

        return portHandler
    except Exception as e:
        print(f"❌ 打开串口错误: {e}")
        return None


# 测试舵机是否在线 (ping)
def ping_servo(portHandler, servo_id):
    packetHandler = sms_sts(portHandler)
    model_number, result, error = packetHandler.ping(servo_id)
    if result == COMM_SUCCESS:
        print(f"✅ 舵机 [ID:{servo_id}] 在线, 型号: {model_number}")
        return True
    else:
        print(f"❌ 舵机 [ID:{servo_id}] 不可用 (错误码: {result})")
        return False


# 将角度变成舵机用的位置
def angle_to_pos(angle):
    return int(angle * 4096 / 360)


# 控制两个舵机在范围内来回运动
def swing_two_servos(portHandler, ids, pos1, pos2, speed=200, acc=50, tol=10):
    packetHandler = sms_sts(portHandler)
    print(f"开始控制舵机 {ids[0]} 和 {ids[1]} 在 {pos1} ↔ {pos2} 之间往复运动 (Ctrl+C 停止)")

    try:
        direction = True  # True 表示 pos1 → pos2, False 表示 pos2 → pos1
        while True:
            # 目标位置
            target = pos2 if direction else pos1

            # 下发目标位置
            for sid in ids:
                packetHandler.WritePosEx(sid, target, speed, acc)

            # 记录开始时间
            t0 = time.time()
            # 循环读取直到到达目标
            while True:
                all_reached = True
                for sid in ids:
                    pos, _, _ = packetHandler.ReadPos(sid)
                    speed_val, _, _ = packetHandler.ReadSpeed(sid)
                    now = time.time()
                    # print(f"[ID:{sid}] Pos={pos}, Speed={speed_val}, Time={now:.3f}")

                    if abs(pos - target) > tol:  # 还没到目标
                        all_reached = False

                if all_reached:
                    t1 = time.time()
                    print(f"✅ 舵机到达目标 {target}，耗时 {t1 - t0:.3f} 秒\n")
                    break

                time.sleep(0.01)  # 50ms 采样一次，避免占用过高

            direction = not direction

    except KeyboardInterrupt:
        print("\n⏹️ 已停止舵机运动")


# 使用 SyncWritePosEx 同步写入两个舵机目标位置
def swing_two_servos_sync(portHandler, ids, pos1, pos2, speed=0, acc=0, delay=1.0):
    packetHandler = sms_sts(portHandler)
    print(f"开始同步控制舵机 {ids[0]} 和 {ids[1]} 在 {pos1} ↔ {pos2} 之间往复运动 (Ctrl+C 停止)")

    try:
        direction = True
        while True:
            # 先清空上一次缓存
            packetHandler.groupSyncWrite.clearParam()

            if direction:
                # 第一个舵机去 pos1, 第二个舵机去 pos2
                packetHandler.SyncWritePosEx(ids[0], pos1, speed, acc)
                packetHandler.SyncWritePosEx(ids[1], pos2, speed, acc)
            else:
                # 第一个舵机去 pos2, 第二个舵机去 pos1
                packetHandler.SyncWritePosEx(ids[0], pos2, speed, acc)
                packetHandler.SyncWritePosEx(ids[1], pos1, speed, acc)

            # 一次性发送同步指令
            packetHandler.groupSyncWrite.txPacket()

            direction = not direction
            time.sleep(delay)

    except KeyboardInterrupt:
        print("\n⏹️ 已停止舵机运动")


def read_servo_feedback(portHandler, servo_id):
    packetHandler = sms_sts(portHandler)

    pos, _, _ = packetHandler.ReadPos(servo_id)
    speed, _, _ = packetHandler.ReadSpeed(servo_id)

    now = time.time()  # 当前时间戳 (秒)

    print(f"[ID:{servo_id}] Pos={pos}, Speed={speed}, Time={now:.3f}")
    return pos, speed, now


if __name__ == "__main__":
    # 1. 列出串口
    ports = list_ports()
    if not ports:
        print("❌ 没有检测到串口")
        exit()

    # 2. 用户选择端口
    index = int(input("请输入要打开的端口编号: "))
    
    # 3. 列出波特率，用户选择
    baudrates = list_baudrates()
    baud_choice = input("请输入波特率编号 (默认 0 → 1000000): ")
    if baud_choice.strip() == "":
        baudrate = baudrates[0]   # 默认 1000000
    else:
        baudrate = baudrates[int(baud_choice)]

    # 4. 打开串口
    portHandler = open_port(ports, index, baudrate)
    if portHandler is None:
        exit()

    # 5. 输入舵机 ID
    ids = input("请输入两个舵机的ID (用空格分隔): ").split()
    ids = [int(i) for i in ids]

    # 6. 分别 ping 两个舵机
    online = [ping_servo(portHandler, sid) for sid in ids]
    if not all(online):
        print("❌ 有舵机未响应，请检查供电/接线/ID 设置")
        portHandler.closePort()
        exit()

    # 🆕 让舵机先回到 0 位置
    packetHandler = sms_sts(portHandler)
    for sid in ids:
        packetHandler.WritePosEx(sid, 0, 0, 0)   # 位置=0, 最大速度, 加速度=0
    print("✅ 舵机已复位到 0 位置，请等待舵机停止...")
    time.sleep(2)  # 给舵机一点时间转到 0

    # 7. 输入运动范围
    pos1 = int(input("请输入第一个位置 (pos1): "))
    pos2 = int(input("请输入第二个位置 (pos2): "))

    #code1: 舵机1和舵机2是转到相同的位置,然后是异步写入   先写一个,在写另一个
    swing_two_servos(portHandler, ids, pos1, pos2, speed=0, acc=0, tol=10)

    # #code2: 舵机1和舵机2是先写入，然后在同时发送
    # swing_two_servos_sync(portHandler, ids, pos1, pos2, speed=0, acc=0, delay=1.5)


    # 9. 关闭端口
    portHandler.closePort()
    print("✅ 通信结束，端口已关闭")
